/*
******************************************************************************************************* 
**  Copyright (C) 2019, 苏州检易生物科技有限公司 
**  All rights reserved. 
** 
**  FileName:       	can_task.c
**  Description:		fpga e transmit protocol	FPGA与中位机电机协议
**  Author:        	 
**  Version				v0.1
**  Date:           	2019-11-26
**	Function List： 
**  History:         
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                              	调试开关
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/
#include "can_task.h"

/*
*********************************************************************************************************
*                                              	宏定义
*********************************************************************************************************
*/
#define		CAN_TASK_STK_SIZE			2048
#define		CAN_TASK_MB_SIZE			32


#define		CAN_TASK_TMR_PERIOD			(10)//2
/*
*********************************************************************************************************
*                                              	结构体定义
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                              	函数原型声明
*********************************************************************************************************
*/
static void can_task_tmr_callback(void* p_arg);

static void can_task_isr_proc(can_bus_t type,app_msg_t* p_msg);

static void can_send_ticks(void);

static void periph_send_ticks(void);

static void can_task_push_proc(can_bus_t type,app_msg_t *p_msg);

static void can_task_process(void *p_arg);

static void can_send_idle(void);

void can_rx_callback(struct BSP_CAN_HANDLE* handle,uint16_t id, uint8_t* buf, uint8_t size);
void can_tx_callback(struct BSP_CAN_HANDLE* handle);
/*
*********************************************************************************************************
*                                              	全局变量定义
*********************************************************************************************************
*/
can_task_handle_t		g_can_task;
extern __IO uint64_t LocalTime;
/*
*********************************************************************************************************
*                                              	模块静态变量定义
*********************************************************************************************************
*/
/*模块初始化静态变量*/
static struct rt_thread   	rt_can_tcb;
static char									can_task_stk[CAN_TASK_STK_SIZE];
static char									can_task_name[]="can task\0";
static char									can_task_mb_pool[CAN_TASK_MB_SIZE * 4];
static rt_timer_t						p_can_timer;


static can_mem_list_t	m_can_send_list;
static can_mem_list_t	m_can_ack_list;
static can_mem_list_t	m_periph_send_list;
/*
*********************************************************************************************************
*                                              	函数定义
*********************************************************************************************************
*/
void can_task_start(void)
{
	if(RT_EOK != rt_thread_init(&rt_can_tcb,
		can_task_name, 
		can_task_process,
		RT_NULL,
		can_task_stk,
		CAN_TASK_STK_SIZE,
		CAN_TASK_PRIO,
		RT_THREAD_TIME_SLICE))
	{
		Critical_Log("%s create failed!\r\n",can_task_name);
//		while(1){}
	}
	else
		rt_thread_startup(&rt_can_tcb);
}
/*
*********************************************************************************************************
*					
*********************************************************************************************************
*/
static void can_task_init(void)
{
	/*创建消息邮箱*/
	if(RT_EOK != rt_mb_init(&g_can_task.mailbox,
		"can mailbox",
		can_task_mb_pool,
		CAN_TASK_MB_SIZE,
		RT_IPC_FLAG_FIFO
		))
	{
		Critical_Log("%s create failed!\r\n",can_task_name);
	}
	p_can_timer = rt_timer_create("can timer",can_task_tmr_callback,RT_NULL,CAN_TASK_TMR_PERIOD,RT_TIMER_FLAG_PERIODIC);
	if(p_can_timer == NULL)
	{
		Critical_Log("%s create timer failed!\r\n",can_task_name);
	}
	else
		rt_timer_start(p_can_timer);
	memset(&m_can_ack_list,0,sizeof(can_mem_list_t));
	m_can_send_list.max_size = CAN_SEND_LIST_MAX;
	m_can_ack_list.max_size = CAN_ACK_LIST_MAX;
	m_periph_send_list.max_size = CAN_PERIPH_LIST_MAX;
	
	g_can_task.p_tcb = &rt_can_tcb;
	g_can_task.p_send_list = &m_can_send_list;
	g_can_task.p_ack_list = &m_can_ack_list;
	g_can_task.p_periph_list = &m_periph_send_list;
	
	g_can_task.can_handle = bsp_can_init(MOTOR_CAN_CHN,MOTOR_CAN_ID,MOTOR_CAN_BAUDRATE);
	bsp_can_cb_reg(g_can_task.can_handle,can_rx_callback,can_tx_callback);
//#if PRODUCT_NAME_IVD_SAMPLE
//#else
	g_can_task.periph_handle = bsp_can_init(PERIPH_CAN_CHN,PERIPH_CAN_ID,PERIPH_CAN_BAUDRATE);
	bsp_can_cb_reg(g_can_task.periph_handle,can_rx_callback,can_tx_callback);
//#endif
	can_mem_init();
	act_init();
}

/*
*********************************************************************************************************
*					
*********************************************************************************************************
*/
static void can_task_process(void *p_arg)
{
	app_msg_t *p_msg = NULL;
	rt_err_t err;
	
	can_task_init();
	/* 初始化相关外设 */

	Trace_Log("Now can task start!\n");

	while(1)
	{
		if(RT_EOK != rt_mb_recv(&g_can_task.mailbox,(rt_uint32_t*)&p_msg,RT_WAITING_FOREVER)
			|| p_msg == NULL)
		{
			p_msg = NULL;
			continue;
		}
		switch(p_msg->event)
		{
			case CAN_MSG_TMR:
				can_send_ticks();
				periph_send_ticks();
				break;
			case CAN_MSG_ISR:
				can_task_isr_proc(CAN_BUS_MOTOR,p_msg);
				break;
			case CAN_MSG_PUSH:
				can_task_push_proc(CAN_BUS_MOTOR,p_msg);
				break;
			case PERIPH_MSG_ISR:
				can_task_isr_proc(CAN_BUS_PERIPH,p_msg);
				break;
			case PERIPH_MSG_PUSH:
				can_task_push_proc(CAN_BUS_PERIPH,p_msg);
				break;
			default:
				break;
		}
		app_msg_free(p_msg);
	}
}

static void can_task_isr_proc(can_bus_t type,app_msg_t* p_msg)
{
	can_mem_node_t* p_recv = (can_mem_node_t*)(p_msg->data);
	can_mem_node_t* p_ack = NULL;
	
	/*用于CAN驱动板的追踪命令*/
	if( (p_recv->can_msg.can_id & CAN_ID_FN_MSK) == FN_DR_TR )
	{
			Critical_Log("Can bus receive debug message:id:%04X;dlc:%d;data:0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X!\r\n"
				,p_recv->can_msg.can_id,p_recv->can_msg.can_dlc,
				p_recv->can_msg.can_data[0],p_recv->can_msg.can_data[1],p_recv->can_msg.can_data[2],p_recv->can_msg.can_data[3],
				p_recv->can_msg.can_data[4],p_recv->can_msg.can_data[5],p_recv->can_msg.can_data[6],p_recv->can_msg.can_data[7]);
		can_mem_free(p_recv);
		return;
	}
	
	if(type == CAN_BUS_MOTOR)
		p_ack = g_can_task.p_ack_list->p_head;
	else if(type == CAN_BUS_PERIPH)
		p_ack = g_can_task.p_periph_list->p_head;
	if(is_sdo_cmd(&p_recv->can_msg))
	{
		if( (p_ack == NULL)
			|| ((p_ack != NULL) && (CAN_ID_GET_NODE(p_ack->can_msg.can_id) != CAN_ID_GET_NODE(p_recv->can_msg.can_id)))
			)
		{
			Critical_Log("Can bus receive unexpect message:id:%04X;dlc:%d;data:0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X!\r\n"
				,p_recv->can_msg.can_id,p_recv->can_msg.can_dlc,
				p_recv->can_msg.can_data[0],p_recv->can_msg.can_data[1],p_recv->can_msg.can_data[2],p_recv->can_msg.can_data[3],
				p_recv->can_msg.can_data[4],p_recv->can_msg.can_data[5],p_recv->can_msg.can_data[6],p_recv->can_msg.can_data[7]);
			can_mem_free(p_recv);
			return;
		}
	}
	
	uint8_t sdo_cs;
	if(is_sdo_cmd(&p_recv->can_msg) && p_ack != NULL && is_sdo_cmd(&p_ack->can_msg))
	{
		sdo_cs = p_recv->can_msg.can_data[0];
		if(sdo_cs == SDO_CS_WRITE_ACK)
		{
			p_recv->can_msg.can_dlc = 3;
			p_recv->can_msg.can_data[1] = p_ack->can_msg.can_data[1];
			p_recv->can_msg.can_data[2] = p_ack->can_msg.can_data[2];
		}
		if(type == CAN_BUS_MOTOR)
		{
			can_pct_rx_callback(CAN_BUS_MOTOR,p_recv->can_msg.can_id,p_recv->can_msg.can_dlc,p_recv->can_msg.can_data,CAN_BUS_ERR_NONE);
			can_mem_list_free(g_can_task.p_ack_list,p_ack);
		}
		else if(type == CAN_BUS_PERIPH)
		{
			can_pct_rx_callback(CAN_BUS_PERIPH,p_recv->can_msg.can_id,p_recv->can_msg.can_dlc,p_recv->can_msg.can_data,CAN_BUS_ERR_NONE);
			can_mem_list_free(g_can_task.p_periph_list,p_ack);
			g_can_task.periph_ack = 0;
		}
	}
	else /*不是一发一回的帧*/
	{
		if(type == CAN_BUS_MOTOR)
			can_pct_rx_callback(CAN_BUS_MOTOR,p_recv->can_msg.can_id,p_recv->can_msg.can_dlc,p_recv->can_msg.can_data,CAN_BUS_ERR_NONE);
		else if(type == CAN_BUS_PERIPH)
			can_pct_rx_callback(CAN_BUS_PERIPH,p_recv->can_msg.can_id,p_recv->can_msg.can_dlc,p_recv->can_msg.can_data,CAN_BUS_ERR_NONE);
	}
	can_mem_free(p_recv);
	/*在CAN总线空闲时 调用一些其他不紧急的CAN总线功能*/
	if(m_can_send_list.cur_size == 0)
	{
		can_send_idle();
	}
	if(m_can_send_list.cur_size == 0
		&& m_periph_send_list.cur_size == 0)
		return;
	if(app_msg_send(CAN_MSG_TMR, NULL, &g_can_task.mailbox) != 0)
	{
		Trace_Log("can task timer send fail!\n");
	}
}
/*在CAN总线空闲时 调用一些其他不紧急的CAN总线功能 比如温控 电机状态轮询 */
static void can_send_idle(void)
{
//	motor_poll_all();
}
static void can_send_ticks(void)
{
	static volatile uint64_t motor_tick;
	/*硬件TX就绪*/
	if(bsp_can_tx_available(g_can_task.can_handle) != CAN_ERR_NONE)
		return;
	rt_base_t it = rt_hw_interrupt_disable();
	/*ack超时重发*/
	if(g_can_task.p_ack_list->cur_size)
	{
		if(LocalTime - motor_tick > CAN_TASK_ACK_TIMEOUT)
		{
			Critical_Log("Can device ack timeout,PSR:%08X;device id:%03X;\r\n",g_can_task.can_handle->hal_can_handle.Instance->PSR,g_can_task.p_ack_list->p_head->can_msg.can_id);
			if(g_can_task.p_send_list->cur_size == CAN_SEND_LIST_MAX)
			{
				Critical_Log("Can send list full!\r\n");
				while(1);
			}
			/*在一发一收情况下重发*/
			can_mem_node_t* p_resend = can_mem_list_pop(g_can_task.p_ack_list);
			p_resend->next = g_can_task.p_send_list->p_head;
			g_can_task.p_send_list->p_head = p_resend;
			g_can_task.p_send_list->cur_size++;
		}
	}
	rt_hw_interrupt_enable(it);
	can_mem_node_t* p_node;

	if(g_can_task.p_send_list->cur_size)
	{
		/*如果下一个待发送为SDO cmd 且acklist满 则中断发送*/
		if(g_can_task.p_ack_list->cur_size == CAN_ACK_LIST_MAX
			&& is_sdo_cmd(&g_can_task.p_send_list->p_head->can_msg))
			return;
		p_node = can_mem_list_pop(g_can_task.p_send_list);
		bsp_can_send(g_can_task.can_handle,p_node->can_msg.can_id,p_node->can_msg.can_data,p_node->can_msg.can_dlc);
		if(is_sdo_cmd(&p_node->can_msg))
		{
			motor_tick = LocalTime;
			can_mem_list_push(g_can_task.p_ack_list,p_node);
		}
		else
			can_mem_free(p_node);
		can_pct_tx_callback(CAN_BUS_MOTOR,p_node->can_msg.can_id,p_node->can_msg.can_dlc,p_node->can_msg.can_data,CAN_BUS_ERR_NONE);
	}
}

static void periph_send_ticks(void)
{
	static uint8_t timeout_cnt = 0; /*总线超时计数,超过一定次数判定为设备超时*/
	static volatile uint64_t tick;
	can_mem_node_t* p_node;
	uint16_t can_id;
	/*硬件TX就绪*/
	if(bsp_can_tx_available(g_can_task.periph_handle) != CAN_ERR_NONE)
		return;
	
	rt_base_t it = rt_hw_interrupt_disable();
	/*ack超时重发*/
	if(g_can_task.periph_ack)
	{
		if(LocalTime - tick > CAN_TASK_ACK_TIMEOUT)
		{
			Critical_Log("Can peripheral device ack timeout,PSR:%08X;device id:%03X;\r\n",g_can_task.periph_handle->hal_can_handle.Instance->PSR,
				g_can_task.p_periph_list->p_head->can_msg.can_id);
			if(timeout_cnt < CAN_PERIPH_MAX_RETRANS)
			{
				timeout_cnt += 2;
			}
			else
			{
				timeout_cnt = 0;
				can_pct_rx_callback(CAN_BUS_PERIPH,g_can_task.p_periph_list->p_head->can_msg.can_id,g_can_task.p_periph_list->p_head->can_msg.can_dlc,
					g_can_task.p_periph_list->p_head->can_msg.can_data,CAN_BUS_ERR_RX_TIMEOUT);
				/*超时的话,删除所有该节点的CAN msg*/
				can_id = g_can_task.p_periph_list->p_head->can_msg.can_id;
				p_node = g_can_task.p_periph_list->p_head;
				while(p_node != NULL)
				{
					if(p_node->can_msg.can_id == can_id)
					{
						can_mem_list_free(g_can_task.p_periph_list,p_node);
						p_node = g_can_task.p_periph_list->p_head;
					}
					else
					{
						p_node = p_node->next;
					}
				}
				if(g_can_task.p_periph_list->cur_size == CAN_PERIPH_LIST_MAX)
				{
	//				while(1)
	//				{
	//					rt_thread_delay(1000);
	//				}
				}
			}
			/*在一发一收情况下重发*/
			g_can_task.periph_ack --;
		}
	}
	rt_hw_interrupt_enable(it);

	if(g_can_task.p_periph_list->cur_size)
	{
		/*如果下一个待发送为SDO cmd 且总线正在等待sdo回复 则不发送*/
		if( g_can_task.periph_ack != 0
			&& is_sdo_cmd(&g_can_task.p_periph_list->p_head->can_msg))
			return;
		if(timeout_cnt)timeout_cnt--;
		p_node = g_can_task.p_periph_list->p_head;
		bsp_can_send(g_can_task.periph_handle,p_node->can_msg.can_id,p_node->can_msg.can_data,p_node->can_msg.can_dlc);
		can_pct_tx_callback(CAN_BUS_PERIPH,p_node->can_msg.can_id,p_node->can_msg.can_dlc,p_node->can_msg.can_data,CAN_BUS_ERR_NONE);
		if(is_sdo_cmd(&p_node->can_msg))
		{
			tick = LocalTime;
			g_can_task.periph_ack++;
		}
		else
		{
			can_mem_list_free(g_can_task.p_periph_list,p_node);
//			can_mem_free(p_node);
		}
	}
}

static void can_task_push_proc(can_bus_t type,app_msg_t *p_msg)
{
	ASSERT(p_msg);
	if(type == CAN_BUS_MOTOR)
	{
		can_mem_node_t* p_recv = (can_mem_node_t*)(p_msg->data);
		can_mem_list_push(g_can_task.p_send_list,p_recv);
		if(g_can_task.p_ack_list->cur_size)
			return;
		if(app_msg_send(CAN_MSG_TMR, NULL, &g_can_task.mailbox) != 0)
		{
			Trace_Log("can task timer send fail!\n");
		}
	}
	else if(type == CAN_BUS_PERIPH)
	{
		can_mem_node_t* p_recv = (can_mem_node_t*)(p_msg->data);
		can_mem_list_push(g_can_task.p_periph_list,p_recv);
		if(g_can_task.p_periph_list->cur_size)
			return;
		if(app_msg_send(CAN_MSG_TMR, NULL, &g_can_task.mailbox) != 0)
		{
			Trace_Log("can task timer send fail!\n");
		}
	}
}

static void can_task_tmr_callback(void* p_arg)
{
//	(void)p_arg;
//	if(g_can_task.p_send_list->cur_size == 0)
//		can_send_idle();
//	if(g_can_task.p_send_list->cur_size == 0 && g_can_task.p_periph_list->cur_size == 0)
//		return;
	if(app_msg_send(CAN_MSG_TMR, NULL, &g_can_task.mailbox) != 0)
	{
		Trace_Log("can task timer send fail!\n");
	}
}
uint16_t can_get_motor_list_size(void)
{
	return m_can_send_list.cur_size;
}
uint16_t can_get_periphlist_size(void)
{
	return m_periph_send_list.cur_size;
}
/*发送完成中断回调*/
void can_tx_callback(struct BSP_CAN_HANDLE* handle)
{
//	if(g_can_task.can_handle == handle)
//	{
//		if(g_can_task.p_send_list->cur_size == 0)
//		{
//			return;
//		}
//		if(app_msg_send(CAN_MSG_TMR, NULL, &g_can_task.mailbox) != 0)
//		{
//			Trace_Log("can task timer send fail!\n");
//		}
//	}
}
/*接收中断回调*/
void can_rx_callback(struct BSP_CAN_HANDLE* handle,uint16_t id, uint8_t* buf, uint8_t size)
{
	ASSERT(buf);
	rt_base_t it = rt_hw_interrupt_disable();
	can_mem_node_t* p_node = can_mem_malloc();
	if(p_node == NULL)
	{
		Critical_Log("can mem get failed!\n");
		rt_hw_interrupt_enable(it);
		return;	
	}	
	memset(p_node, 0, sizeof(can_mem_node_t));	
	memcpy(&(p_node->can_msg.can_data), buf, size);
	p_node->can_msg.can_dlc = size;
	p_node->can_msg.can_id = id;
//	if(p_node->can_msg.can_dlc == 7 && p_node->can_msg.can_data[1] < 50)
//	{
//		Critical_Log("err!\r\n");
//	}
	if(g_can_task.can_handle == handle && app_msg_send(CAN_MSG_ISR, (uint32_t)p_node, &g_can_task.mailbox) != 0)
	{
		can_mem_free(p_node);
		Critical_Log("Send msg %d to %d fail!\n", CAN_MSG_ISR, &g_can_task.p_tcb);
	}
	else if(g_can_task.periph_handle == handle && app_msg_send(PERIPH_MSG_ISR, (uint32_t)p_node, &g_can_task.mailbox) != 0)
	{
		can_mem_free(p_node);
		Critical_Log("Send msg %d to %d fail!\n", PERIPH_MSG_ISR, &g_can_task.p_tcb);
	}
	rt_hw_interrupt_enable(it);
}